import struct
import serial
from collections import namedtuple

# 定义数据结构
IMUData = namedtuple('IMUData', ['pitch', 'roll', 'yaw', 'temperature'])
FrameData = namedtuple('FrameData', ['imu1', 'imu2', 'imu3', 'imu4', 'data1', 'data2', 'data3', 'data4'])

def parse_imu_data(data_bytes):
    """解析单个IMU的数据（12字节浮点数 + 2字节温度）"""
    # 3个float（每个4字节）和1个uint16（2字节）
    roll, pitch, yaw = struct.unpack('<fff', data_bytes[:12])  # 大端序>，小端序<
    temperature = struct.unpack('<H', data_bytes[12:14])[0]    # 大端序无符号短整型>，小端序无符号整形<
    return IMUData(roll, pitch, yaw, temperature)

def parse_frame(data_bytes):
    """解析一帧数据（66字节）"""
    # 检查帧头
    header = data_bytes[:2]
    if header != b'\xA5\x5A':
        raise ValueError(f"无效的帧头: {header.hex()}")
    
    # 解析4个IMU数据
    imu1 = parse_imu_data(data_bytes[2:16])    # 2-15字节
    imu2 = parse_imu_data(data_bytes[16:30])   # 16-29字节
    imu3 = parse_imu_data(data_bytes[30:44])   # 30-43字节
    imu4 = parse_imu_data(data_bytes[44:58])   # 44-57字节
    
    # 解析4个16位数据 (58-65字节)
    data1, data2, data3, data4 = struct.unpack('<4H', data_bytes[58:66])    # 小端序
    
    return FrameData(imu1, imu2, imu3, imu4, data1, data2, data3, data4)

def read_serial_data(port, baudrate=921600):
    """
    从串口读取并解析数据
    Args:
    ----
    * `port`: COM?
    * `baudrate`: 921600, 115200, etc.
    """
    with serial.Serial(port, baudrate, timeout=1) as ser:
        buffer = bytearray()
        
        while True:
            # 读取数据
            data = ser.read(ser.in_waiting or 1)
            buffer.extend(data)
            
            # 查找帧头
            while len(buffer) >= 66:
                # 查找A55A帧头
                header_pos = buffer.find(b'\xA5\x5A')
                if header_pos == -1:
                    # 没有找到有效帧头，清空缓冲区
                    buffer.clear()
                    break
                
                if header_pos > 0:
                    # 丢弃帧头之前的数据
                    del buffer[:header_pos]
                
                if len(buffer) < 66:
                    # 数据不足一帧，继续接收
                    break
                
                # 提取一帧数据
                frame_data = buffer[:66]
                del buffer[:66]
                
                try:
                    # 解析帧数据
                    parsed = parse_frame(frame_data)
                    yield parsed
                except Exception as e:
                    print(f"解析错误: {e}")
                    continue

# 使用示例
if __name__ == "__main__":
    # 替换为你的串口设备
    # serial_port = '/dev/ttyUSB0'  # Linux/Mac
    serial_port = 'COM22'         # Windows
    
    for frame in read_serial_data(serial_port):
        print("\n=== 新帧数据 ===")
        print(f"IMU1 - 横滚: {frame.imu1.roll:.2f}°, 俯仰: {frame.imu1.pitch:.2f}°, 偏航: {frame.imu1.yaw:.2f}°, 温度: {frame.imu1.temperature}")
        print(f"IMU2 - 横滚: {frame.imu2.roll:.2f}°, 俯仰: {frame.imu2.pitch:.2f}°, 偏航: {frame.imu2.yaw:.2f}°, 温度: {frame.imu2.temperature}")
        print(f"IMU3 - 横滚: {frame.imu3.roll:.2f}°, 俯仰: {frame.imu3.pitch:.2f}°, 偏航: {frame.imu3.yaw:.2f}°, 温度: {frame.imu3.temperature}")
        print(f"IMU4 - 横滚: {frame.imu4.roll:.2f}°, 俯仰: {frame.imu4.pitch:.2f}°, 偏航: {frame.imu4.yaw:.2f}°, 温度: {frame.imu4.temperature}")
        print(f"附加数据: {frame.data1}, {frame.data2}, {frame.data3}, {frame.data4}")